{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "gToHma1nvZxz",
   "metadata": {
    "id": "gToHma1nvZxz"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 8a</h4>\n",
    "<font color=blue><h1>Fundamental Limits for Control of a Magnetic Levitation System</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1MuDZfw72UkI4_Ji_AsEDTPi7IaSURsYP)\n",
    "\n",
    "This notebook contains the code used to create the magnetic levitation example in Lecture 8-1 of CDS 110, Winter 2024."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dc288b3e-60cc-4a75-8af5-81f9d1eede41",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import scipy as sp\n",
    "import matplotlib.pyplot as plt\n",
    "from math import pi\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct\n",
    "import control.optimal as opt\n",
    "import control.flatsys as fs"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "RFi9litmZKT2",
   "metadata": {
    "id": "RFi9litmZKT2"
   },
   "source": [
    "The magnetic leviation system consists of a metal ball, an electromagnet, and an IR sensor:\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/maglev-diagram.png\", alt=\"maglev-diagram\" width=400></center>\n",
    "\n",
    "It is governed by following equation:\n",
    "\n",
    "$$ \\ddot{z} = g - \\frac{k_mk_A^2}{m}\\frac{u^2}{z^2} - \\frac{c}{m}\\dot{z},$$\n",
    "\n",
    "where $z$ is the vertical height of the ball and $u$ is the input current applied to the electromagnet.  The output is given by $v_{ir}$, which is the voltage measured at the IR sensor:\n",
    "\n",
    "$$v_{ir} = k_T z + v_0 $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "80da9750-1a34-4a54-ab3a-ff37ea7be0f6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# System dynamics\n",
    "maglev_params = {\n",
    "    'kT': 613.65,      # gain between position and voltage\n",
    "    'v0': -16.18,\t   # voltage offset at zero position\n",
    "    'm': 0.2,\t       # mass of ball, kg\n",
    "    'g': 9.81,         # gravitational constant\n",
    "    'kA': 1,\t       # electromagnet conductance\n",
    "    'c': 1             # damping (added to improve visualization)\n",
    "}\n",
    "# gain on magnetic attractive force\n",
    "maglev_params['km'] = 3.13e-3 * (maglev_params['m']/2) / maglev_params['kA']**2\n",
    "\n",
    "def maglev_update(t, x, u, params):\n",
    "    m, g, kA, km, c = map(params.get, ['m', 'g', 'kA', 'km', 'c'])\n",
    "    return np.array([\n",
    "        x[1],\n",
    "        g - km/m * (kA * u[0])**2 / x[0]**2 - c * x[1]\n",
    "    ])\n",
    "\n",
    "def maglev_output(t, x, u, params):\n",
    "    kT, v0 = map(params.get, ['kT', 'v0'])\n",
    "    return np.array([kT * x[0] + v0])\n",
    "\n",
    "maglev = ct.nlsys(\n",
    "    maglev_update, maglev_output, params=maglev_params, name='maglev',\n",
    "    inputs='Vu', outputs='Vy', states=['pos', 'vel']\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5c56e04-03b7-4c18-be3c-3f4308aedb98",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the equilibrium point that holds the ball at the origin\n",
    "xeq, ueq = ct.find_eqpt(maglev, [0.02, 0], 0.2, y0=0)\n",
    "print(f\"{xeq=}, {ueq=}\", end='\\n----\\n')\n",
    "\n",
    "# Compute the linearization at that point\n",
    "magP = ct.linearize(maglev, xeq, ueq, name='sys')\n",
    "print(magP, end='\\n----\\n')\n",
    "\n",
    "print(\"Poles:\", magP.poles())\n",
    "print(\"Zeros:\", magP.zeros())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22a2766f-217a-4213-ba19-c11485cc42cc",
   "metadata": {},
   "source": [
    "The controller for this system is implemented via an electrical circuit consisting of resistors and capacitors.  We don't show the circuit here, but just write down the model for the transfer function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b4741e88-bedd-4ef0-b8b9-9deb5fa93d5d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Controller (analog circuit)\n",
    "k1 = 0.5\t\t\t\t# gain set by gain pot\n",
    "R1 = 22000\t\t\t\t# Internal resistor\n",
    "R2 = 22000\t\t\t\t# Resistor plug-in\n",
    "R = 2000; C = 1e-6\t\t# RC plug-in\n",
    "\n",
    "# Controller based on analog circuit\n",
    "magC1 = -ct.tf([(R1 + R) * C, 1], [R * C, 1]) * k1 * R2/R1\n",
    "magL1 = magP * magC1"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "641c0df2-90f6-4573-af7f-41a305337e77",
   "metadata": {},
   "source": [
    "We can now use a Nyquist plot to see if the controller is stabilizing:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "378b14b8-f8e4-4ed6-b09d-cdf577ea47d1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Nyquist plot\n",
    "cplt = ct.nyquist_plot([magP, magL1], label=[\"sys\", \"sys * ctrl\"])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "HKGSdW5f91mZ",
   "metadata": {
    "id": "HKGSdW5f91mZ"
   },
   "source": [
    "We see that the controller causes the system to have clockwise net encircelement of the origin.  Since the open loop system has one unstable pole, this gives $Z = N + P = 0$ and so the closed loop system is stable."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7850f14d-79ab-4250-a0c7-8ddc10ebb977",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Bode plots\n",
    "magC1.name = \"ctrl\"\n",
    "cplt = ct.bode_plot(\n",
    "    [magP, magC1, magL1], np.logspace(0, 4), initial_phase=0,\n",
    "    label=['P', 'C', 'L'])\n",
    "cplt.axes[0, 0].set_ylim(0.06, 1.5e1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d83c5d5c-238a-45a1-9a81-a3779e7f7bc3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Sensitivity function for closed loop system/.\n",
    "magS1 = ct.feedback(1, magL1, name=\"S1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3bdcb116-02fd-46d9-ab4d-5b25511d0b21",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Step response\n",
    "magT1 = ct.feedback(magL1, name=\"T1\")\n",
    "ct.step_response(magT1).plot(title=\"Step response for closed loop system\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2ddb53c-023b-466b-ac15-221c22befd6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Try to improve performance by increasing DC gain\n",
    "# System with gain increased\n",
    "magC2 = magC1*5 \t\t\t                        # increased gain\n",
    "magL2 = magP * magC2 \t\t\t                    # loop transfer function\n",
    "magS2 = ct.feedback(1, magP * magC2, name=\"S2\") \t# sensitivity function\n",
    "magT2 = ct.feedback(magP * magC2, 1, name=\"T2\") \t# closed loop response\n",
    "\n",
    "# System with gain increased even more\n",
    "magC3 = magC1*20\t\t\t                        # increased gain\n",
    "magL3 = magP*magC3\t\t\t                        # loop transfer function\n",
    "magS3 = ct.feedback(1, magP * magC3, name=\"S3\")\t    # sensitivity function\n",
    "magT3 = ct.feedback(magP * magC3, 1, name=\"T3\")\t    # closed loop response\n",
    "\n",
    "# Plot step responses for different systems\n",
    "colors = ['b', 'g', '#FF7F50']\n",
    "for sys in [magT1, magT2, magT3]:\n",
    "    ct.step_response(sys).plot(color=colors.pop())\n",
    "\n",
    "# Bode plot for sensitivity function\n",
    "plt.figure()\n",
    "cplt = ct.bode_plot([magS1, magS2, magS3], plot_phase=False)\n",
    "\n",
    "# Add magnitude of 1\n",
    "xdata = cplt.lines[0][0][0].get_xdata()\n",
    "ydata = np.ones_like(xdata)\n",
    "plt.plot(xdata, ydata, color='k', linestyle='--');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4df561a2-16aa-41b0-9971-f8c151467730",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Bode integral calculation\n",
    "omega = np.linspace(0, 1e6, 100000)\n",
    "for name, sys in zip(['C1', 'C2', 'C3'], [magS1, magS2, magS3]):\n",
    "    freqresp = ct.frequency_response(sys, omega)\n",
    "    bodeint = np.trapz(np.log(freqresp.magnitude), omega)\n",
    "    print(\"Bode integral for\", name, \"=\", bodeint)\n",
    "\n",
    "print(\"pi * sum[ Re(pk) ]\", pi * np.sum(magP.poles()[magP.poles().real > 0]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "M2EvTYHq8yRb",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
